use std::time::Duration;

use hi32::hi32::{data_utils::{parse_eular, parse_llm}, parser::FrameParser};

fn main() {
    const SER: &str = "COM10";

    let mut port = serialport::new(SER, 115200)
        .timeout(Duration::from_secs(100))
        .open()
        .unwrap();

    port.write_all(b"LOG HI81 ONTIME 1 \r\n").unwrap();
    std::thread::sleep(Duration::from_millis(500));

    let mut buffer = [0u8; 1024];
    let mut parser = FrameParser::default();

    loop {
        let n = port.read(&mut buffer).unwrap();

        parser.parse_buffer(&buffer[0..n], |fr| {
            let data = &fr[6..];
            let (r, p, y) = parse_eular(&data[42..]);
            let (lon, lat, msl) = parse_llm(&data[56..]);

            println!("yaw: {:.2} \t pitch: {:.2} \t roll: {:.2} \t lon: {:.2} \t lat: {:.2} \t msl: {:.2}", y, p, r, lon, lat, msl);
        });
    }
}



